/**
* File: waypoint.cpp
*
* \brief Waypoint structures and miscellaneous functions.
*
* \author Instituto Superior Tecnico de Lisboa, Portugal
* \author Technical University of Lisbon, Portugal
* 
* \author Agentes Autonomos e Sistemas Multiagente
* \author Autonomous Agents and Multiagent Systems
* 
* \version	1.0
* \date		2006/2007
**/
#include "waypoint.h"
#include <vector>
#include <assert.h>
#include "../modApi.h"
#include "../util/util.h"
#include "../util/mathLibrary.h"
#include "../util/language.h"
#include "globals.h"
#include "search.h"
#include "gui.h"
#include "World.h"
using namespace game;
using namespace game::search;
using namespace game::waypoints;

Waypoint *game::waypoints::g_waypoints[MAX_WAYPOINTS];

int *game::waypoints::g_terroristWaypoints = NULL;
int game::waypoints::g_numTerroristWaypoints = 0;
int *game::waypoints::g_counterTerroristWaypoints = NULL;
int game::waypoints::g_numCounterTerroristWaypoints = 0;
int *game::waypoints::g_goalWaypoints = NULL;
int game::waypoints::g_numGoalWaypoints = 0;
int *game::waypoints::g_campWaypoints = NULL;
int game::waypoints::g_numCampWaypoints = 0;
int *game::waypoints::g_rescueWaypoints = NULL;
int game::waypoints::g_numRescueWaypoints = 0;

bool game::waypoints::g_editNoclip = false;
cvar_t game::waypoints::g_cvarWPTDirname;

/* Whether any waypoints have been allocated */
bool g_bWaypointsAllocated = false;  
bool g_bEndJumpPoint = false;
float g_fTimeJumpStarted = 0.0;
Vector g_vecLearnVelocity = g_vecZero;
Vector g_vecLearnPos = g_vecZero;
int g_iLastJumpWaypoint = -1;
Vector g_vecLastWaypoint;
float g_fPathTime = 0.0;
float g_flDangerTime = 0.0;
/* Time this waypoint was displayed (while editing) */
float g_rgfWPDisplayTime[MAX_WAYPOINTS];
unsigned char g_rgbyVisLUT[MAX_WAYPOINTS][MAX_WAYPOINTS / 4];

bool game::waypoints::g_waypointOn = false;
bool game::waypoints::g_waypointsChanged = true;
bool game::waypoints::g_autoWaypointOn = false;
bool game::waypoints::g_learnJumpWaypoint = false;
int game::waypoints::g_findWPIndex = -1;
int game::waypoints::g_cacheWaypointIndex = -1;
int game::waypoints::g_numWaypoints;

char game::waypoints::g_waypointer[32];

/**
* \brief Resets the waypoint structures. 
*
* Clears all waypointing information.
**/
void game::waypoints::reset(void)
{
   // have any waypoint path nodes been allocated yet?
   if (g_bWaypointsAllocated)
   {
      // must free previously allocated path memory
      for (int i = 0; i < g_numWaypoints; i++)
      {
         free(g_waypoints[i]);
         g_waypoints[i] = NULL;
      }
   }

   g_numWaypoints = 0;
   g_vecLastWaypoint = g_vecZero;
}

/**
* \brief Initializes waypoint type specific buffers and variables.
**/
void game::waypoints::initializeTypes()
{
   g_numTerroristWaypoints = 0;
   g_numCounterTerroristWaypoints = 0;
   g_numGoalWaypoints = 0;
   g_numCampWaypoints = 0;
   g_numRescueWaypoints = 0;

   if (g_terroristWaypoints)
      free(g_terroristWaypoints);
   g_terroristWaypoints = NULL;

   if (g_counterTerroristWaypoints)
      free(g_counterTerroristWaypoints);
   g_counterTerroristWaypoints = NULL;

   if (g_goalWaypoints)
      free(g_goalWaypoints);
   g_goalWaypoints = NULL;

   if (g_campWaypoints)
      free(g_campWaypoints);
   g_campWaypoints = NULL;

   if (g_rescueWaypoints)
      free(g_rescueWaypoints);
   g_rescueWaypoints = NULL;

   for (int index = 0; index < g_numWaypoints; index++)
   {
	   // Update 1: Distinguish important/camp wpts: Start
      if ((g_waypoints[index]->flags & WaypointFlags::terrorist) &&
         !(g_waypoints[index]->flags & WaypointFlags::camp))
	   // Update 1: End
      {
         if (g_numTerroristWaypoints <= 0)
            g_terroristWaypoints = (int *)malloc(sizeof(int));
         else
            g_terroristWaypoints = (int *)realloc(g_terroristWaypoints, sizeof(int) * (g_numTerroristWaypoints + 1));

         if (!g_terroristWaypoints)
            util::terminateOnError("Memory Allocation Error!");

         g_terroristWaypoints[g_numTerroristWaypoints] = index;
         g_numTerroristWaypoints++;
      }
      else if ((g_waypoints[index]->flags & WaypointFlags::counterTerrorist) &&
         !(g_waypoints[index]->flags & WaypointFlags::camp))
      {
         if (g_numCounterTerroristWaypoints <= 0)
            g_counterTerroristWaypoints = (int *)malloc(sizeof(int));
         else
            g_counterTerroristWaypoints = (int *)realloc(g_counterTerroristWaypoints, sizeof(int) * (g_numCounterTerroristWaypoints + 1));

         if (!g_counterTerroristWaypoints)
            util::terminateOnError("Memory Allocation Error!");

         g_counterTerroristWaypoints[g_numCounterTerroristWaypoints] = index;
         g_numCounterTerroristWaypoints++;
      }
      else if (g_waypoints[index]->flags & WaypointFlags::goal)
      {
         if (g_numGoalWaypoints <= 0)
            g_goalWaypoints = (int *)malloc(sizeof(int));
         else
            g_goalWaypoints = (int *)realloc(g_goalWaypoints, sizeof(int) * (g_numGoalWaypoints + 1));

         if (!g_goalWaypoints)
            util::terminateOnError("Memory Allocation Error!");

         g_goalWaypoints[g_numGoalWaypoints] = index;
         g_numGoalWaypoints++;
      }
      else if (g_waypoints[index]->flags & WaypointFlags::camp)
      {
         if (g_numCampWaypoints <= 0)
            g_campWaypoints = (int *)malloc(sizeof(int));
         else
            g_campWaypoints = (int *)realloc(g_campWaypoints, sizeof(int) * (g_numCampWaypoints + 1));

         if (!g_campWaypoints)
            util::terminateOnError("Memory Allocation Error!");

         g_campWaypoints[g_numCampWaypoints] = index;
         g_numCampWaypoints++;
      }
      else if (g_waypoints[index]->flags & WaypointFlags::rescue)
      {
         if (g_numRescueWaypoints <= 0)
            g_rescueWaypoints = (int *)malloc(sizeof(int));
         else
            g_rescueWaypoints = (int *)realloc(g_rescueWaypoints, sizeof(int) * (g_numRescueWaypoints + 1));

         if (!g_rescueWaypoints)
            util::terminateOnError("Memory Allocation Error!");

         g_rescueWaypoints[g_numRescueWaypoints] = index;
         g_numRescueWaypoints++;
      }
   }

   //Bug 06: Incorrect waypoint count:: Start
   //g_numTerroristWaypoints--;
   //g_numCounterTerroristWaypoints--;
   //g_numGoalWaypoints--;
   //g_numCampWaypoints--;
   //g_numRescueWaypoints--;
   //Bug 06: End
}

/**
* \brief Loads the waypoints for the current map.
**/
bool game::waypoints::load()
{
   char filename[256];
   WaypointHeader header;
   int index;
   bool bOldWaypointFormat = false;

   g_fPathTime = 0.0;
   g_flDangerTime = 0.0;

   strcpy(filename, STRING(gpGlobals->mapname));

   char buffer[64];
   getServerCommandPrefix(buffer);
   strcat(buffer, "_wptfolder");
   util::getModDirectory(filename);
   strcat(filename, CVAR_GET_STRING(buffer));
   //strcat(filename, CVAR_GET_STRING("aasm2_wptfolder"));
   util::createPath(filename);
   strcat(filename, "/");
   strcat(filename, STRING(gpGlobals->mapname));
   strcat(filename, ".pwf");

   FILE *bfp = fopen(filename, "rb");

   if (bfp != NULL) // if file exists, read the waypoint structure from it
   {
      fread(&header, sizeof(header), 1, bfp);

      header.filetype[7] = 0;
      if (strcmp(header.filetype, "PODWAY!") == 0)
      {
         if (header.waypoint_file_version != WAYPOINT_VERSION)
         {
            if (header.waypoint_file_version == WAYPOINT_VERSION5 ||
               header.waypoint_file_version == WAYPOINT_VERSION6)
               bOldWaypointFormat = true;
            else
            {
               ALERT( at_console, "%s - incorrect waypoint file version\n", filename );
               fclose(bfp);
               return false;
            }
         }

         game::waypoints::reset();  // remove any existing waypoints
         g_numWaypoints = header.number_of_waypoints;

         // store the waypointer name
         strcpy(g_waypointer, header.waypointer);

         // read and add waypoint g_waypoints...
         for (index = 0; index < g_numWaypoints; index++)
         {
            // It's an old format - convert...
            if (bOldWaypointFormat)
            {
               // Oldest Format to convert
               if (header.waypoint_file_version == WAYPOINT_VERSION5)
               {
                  Waypoint5 convpath;

                  g_waypoints[index] = (Waypoint *)malloc(sizeof(Waypoint));
                  if (g_waypoints[index] == NULL)
                     util::terminateOnError("Memory Allocation Error!");

                  // read 1 WaypointStruct5
                  fread(&convpath, sizeof(Waypoint5), 1, bfp);
                  // Convert old to new
                  g_waypoints[index]->pathNumber = convpath.pathNumber;
                  g_waypoints[index]->flags = convpath.flags;
                  g_waypoints[index]->origin = convpath.origin;
                  g_waypoints[index]->radius = convpath.radius;
                  g_waypoints[index]->campStartX = convpath.campStartX;
                  g_waypoints[index]->campStartY = convpath.campStartY;
                  g_waypoints[index]->campEndX = convpath.campEndX;
                  g_waypoints[index]->campEndY = convpath.campEndY;
                  g_waypoints[index]->index[0] = convpath.index[0];
                  g_waypoints[index]->index[1] = convpath.index[1];
                  g_waypoints[index]->index[2] = convpath.index[2];
                  g_waypoints[index]->index[3] = convpath.index[3];
                  g_waypoints[index]->index[4] = -1;
                  g_waypoints[index]->index[5] = -1;
                  g_waypoints[index]->index[6] = -1;
                  g_waypoints[index]->index[7] = -1;
                  g_waypoints[index]->distance[0] = convpath.distance[0];
                  g_waypoints[index]->distance[1] = convpath.distance[1];
                  g_waypoints[index]->distance[2] = convpath.distance[2];
                  g_waypoints[index]->distance[3] = convpath.distance[3];
                  g_waypoints[index]->distance[4] = 0;
                  g_waypoints[index]->distance[5] = 0;
                  g_waypoints[index]->distance[6] = 0;
                  g_waypoints[index]->distance[7] = 0;
                  g_waypoints[index]->connectflag[0] = 0;
                  g_waypoints[index]->connectflag[1] = 0;
                  g_waypoints[index]->connectflag[2] = 0;
                  g_waypoints[index]->connectflag[3] = 0;
                  g_waypoints[index]->connectflag[4] = 0;
                  g_waypoints[index]->connectflag[5] = 0;
                  g_waypoints[index]->connectflag[6] = 0;
                  g_waypoints[index]->connectflag[7] = 0;
                  g_waypoints[index]->vecConnectVel[0] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[1] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[2] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[3] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[4] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[5] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[6] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[7] = g_vecZero;
               }
               else
               {
                  Waypoint6 convpath;

                  g_waypoints[index] = (Waypoint *)malloc(sizeof(Waypoint));
                  if (g_waypoints[index] == NULL)
                     util::terminateOnError("Memory Allocation Error!");

                  // read 1 WaypointStruct5
                  fread(&convpath, sizeof(Waypoint6), 1, bfp);
                  // Convert old to new
                  g_waypoints[index]->pathNumber = convpath.pathNumber;
                  g_waypoints[index]->flags = convpath.flags;
                  g_waypoints[index]->origin = convpath.origin;
                  g_waypoints[index]->radius = convpath.radius;
                  g_waypoints[index]->campStartX = convpath.campStartX;
                  g_waypoints[index]->campStartY = convpath.campStartY;
                  g_waypoints[index]->campEndX = convpath.campEndX;
                  g_waypoints[index]->campEndY = convpath.campEndY;
                  g_waypoints[index]->index[0] = convpath.index[0];
                  g_waypoints[index]->index[1] = convpath.index[1];
                  g_waypoints[index]->index[2] = convpath.index[2];
                  g_waypoints[index]->index[3] = convpath.index[3];
                  g_waypoints[index]->index[4] = convpath.index[4];
                  g_waypoints[index]->index[5] = convpath.index[5];
                  g_waypoints[index]->index[6] = convpath.index[6];
                  g_waypoints[index]->index[7] = convpath.index[7];
                  g_waypoints[index]->distance[0] = convpath.distance[0];
                  g_waypoints[index]->distance[1] = convpath.distance[1];
                  g_waypoints[index]->distance[2] = convpath.distance[2];
                  g_waypoints[index]->distance[3] = convpath.distance[3];
                  g_waypoints[index]->distance[4] = convpath.distance[4];
                  g_waypoints[index]->distance[5] = convpath.distance[5];
                  g_waypoints[index]->distance[6] = convpath.distance[6];
                  g_waypoints[index]->distance[7] = convpath.distance[7];
                  g_waypoints[index]->connectflag[0] = 0;
                  g_waypoints[index]->connectflag[1] = 0;
                  g_waypoints[index]->connectflag[2] = 0;
                  g_waypoints[index]->connectflag[3] = 0;
                  g_waypoints[index]->connectflag[4] = 0;
                  g_waypoints[index]->connectflag[5] = 0;
                  g_waypoints[index]->connectflag[6] = 0;
                  g_waypoints[index]->connectflag[7] = 0;
                  g_waypoints[index]->vecConnectVel[0] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[1] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[2] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[3] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[4] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[5] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[6] = g_vecZero;
                  g_waypoints[index]->vecConnectVel[7] = g_vecZero;
               }
            }
            else
            {
               g_waypoints[index] = (Waypoint *)malloc(sizeof(Waypoint));
               if (g_waypoints[index] == NULL)
                  util::terminateOnError("Memory Allocation Error!");
               // read the number of g_waypoints from this node...
               fread(g_waypoints[index], sizeof(Waypoint), 1, bfp);
            }
         }

         g_bWaypointsAllocated = true;  // keep track so path can be freed
      }
      else
      {
         ALERT(at_console, "%s is not a POD Agent waypoint file!\n", filename);
         fclose(bfp);
         return false;
      }
      fclose(bfp);
   }
   else
   {
      ALERT( at_console, "Waypoint file %s does not exist!\n", filename );
      return false;
   }

   // Reset Display Times for Waypoint Editing
   for (index = 0; index < g_numWaypoints; index++)
      g_rgfWPDisplayTime[index] = 0.0;

   game::waypoints::initializeTypes();
   if (g_world->isMapInitialized())
      game::waypoints::calculateVisibility();
   game::search::initializeFloydWarshallMatrices();
   g_waypointsChanged = false;

   return true;
}


/**
* \brief Saves the waypoints for the current map.
**/
void game::waypoints::save()
{
   char filename[256];
   WaypointHeader header;
   int i;

   strcpy(header.filetype, "PODWAY!");

   header.waypoint_file_version = WAYPOINT_VERSION;

   header.number_of_waypoints = g_numWaypoints;

   memset(header.mapname, 0, sizeof(header.mapname));
   memset(header.waypointer, 0, sizeof(header.waypointer));
   strncpy(header.mapname, STRING(gpGlobals->mapname), 31);
   header.mapname[31] = 0;
   strcpy(header.waypointer, STRING(g_world->hostEdict->v.netname));

   char buffer[64];
   getServerCommandPrefix(buffer);
   strcat(buffer, "_wptfolder");
   util::getModDirectory(filename);
   strcat(filename, CVAR_GET_STRING(buffer));
   //strcat(filename, CVAR_GET_STRING("aasm2_wptfolder"));
   util::createPath(filename);
   strcat(filename, "/");
   strcat(filename, header.mapname);
   strcat(filename, ".pwf");

   FILE *bfp = fopen(filename, "wb");
   if (bfp)
   {
      // write the waypoint header to the file...
      fwrite(&header, sizeof(header), 1, bfp);

      // save the waypoints...
      for (i = 0; i < g_numWaypoints; i++)
         fwrite(g_waypoints[i], sizeof(Waypoint), 1, bfp);

      fclose(bfp);
   }
   else
      SERVER_PRINT("Error opening .pwf file for writing!\n");
}

/**
*
**/
void game::waypoints::saveOldFormat()
{
   char filename[256];
   WaypointHeader header;
   int i;
   Waypoint *p;
   Waypoint6 convpath;

   strcpy(header.filetype, "PODWAY!");

   header.waypoint_file_version = WAYPOINT_VERSION6;

   header.number_of_waypoints = g_numWaypoints;

   memset(header.mapname, 0, sizeof(header.mapname));
   memset(header.waypointer, 0, sizeof(header.waypointer));
   strncpy(header.mapname, STRING(gpGlobals->mapname), 31);
   header.mapname[31] = 0;
   strcpy(header.waypointer,STRING(g_world->hostEdict->v.netname));

   char buffer[64];
   getServerCommandPrefix(buffer);
   strcat(buffer, "_wptfolder");
   util::getModDirectory(filename);
   strcat(filename, CVAR_GET_STRING(buffer));
   //strcat(filename, CVAR_GET_STRING("aasm2_wptfolder"));
   util::createPath(filename);
   strcat(filename, "/");
   strcat(filename, header.mapname);
   strcat(filename, ".pwf");

   FILE *bfp = fopen(filename, "wb");
   if (bfp)
   {
      // write the waypoint header to the file...
      fwrite(&header, sizeof(header), 1, bfp);

      // save the waypoint g_waypoints...
      for (i = 0; i < g_numWaypoints; i++)
      {
         int t;
         p = g_waypoints[i];

         memset(&convpath, 0, sizeof(Waypoint6));
         convpath.pathNumber = p->pathNumber;
         convpath.flags = p->flags;
         convpath.origin = p->origin;
         convpath.radius = p->radius;
         convpath.campStartX = p->campStartX;
         convpath.campStartY = p->campStartY;
         convpath.campEndX = p->campEndX;
         convpath.campEndY = p->campEndY;
         convpath.index[0] = p->index[0];
         convpath.index[1] = p->index[1];
         convpath.index[2] = p->index[2];
         convpath.index[3] = p->index[3];
         convpath.index[4] = p->index[4];
         convpath.index[5] = p->index[5];
         convpath.index[6] = p->index[6];
         convpath.index[7] = p->index[7];
         convpath.distance[0] = p->distance[0];
         convpath.distance[1] = p->distance[1];
         convpath.distance[2] = p->distance[2];
         convpath.distance[3] = p->distance[3];
         convpath.distance[4] = p->distance[4];
         convpath.distance[5] = p->distance[5];
         convpath.distance[6] = p->distance[6];
         convpath.distance[7] = p->distance[7];

         for (t = 0; t <= 7; t++)
         {
            if (p->connectflag[t] & WaypointConnectionFlags::jump)
               util::hostPrint(g_world->hostEdict, print_console, util::language::translate("WARNING! Jump path not saved from %d to %d\n"), i, p->index[t]);
         }

         fwrite(&convpath, sizeof(Waypoint6), 1, bfp);
      }
      fclose(bfp);
   }
}


/**
* \brief Performs a series of tests on the current waypoints in order to validate them.
*
* Some of the tests are:
* - Whether it is connected to any other waypoint
* - Whether path number differs from its index
* - If it is a camp waypoint, whether the camp-end position is set
* - Whether there are any terrorist important waypoints
* - Whether there are any counter-terrorist important waypoints
* - Etc.
**/
bool game::waypoints::areValid()
{
   int iTPoints = 0;
   int iCTPoints = 0;
   int iGoalPoints = 0;
   int iRescuePoints = 0;
   int iConnections;
   int i, j;

   for (i = 0; i < g_numWaypoints; i++)
   {
      iConnections = 0;

      for (int n = 0; n < MAX_PATH_INDEX; n++)
      {
         if (g_waypoints[i]->index[n] != -1)
         {
            if (g_waypoints[i]->index[n] > g_numWaypoints)
            {
               SERVER_PRINT(util::varArgs("Node %d connected with invalid Waypoint Nr. %d!\n", i, g_waypoints[i]->index[n]));
               return false;
            }
            iConnections++;
            break;
         }
      }

      if (iConnections == 0)
      {
         if (!game::waypoints::isConnected(i))
         {
            SERVER_PRINT(util::varArgs("Node %d isn't connected with any other Waypoint!\n", i));
            return false;
         }
      }

      if (g_waypoints[i]->pathNumber != i)
      {
         SERVER_PRINT(util::varArgs("Node %d pathnumber differs from index!\n", i));
         return false;
      }

      if (g_waypoints[i]->flags & WaypointFlags::camp)
      {
         if (g_waypoints[i]->campEndX == 0.0 && g_waypoints[i]->campEndY == 0.0)
         {
            SERVER_PRINT(util::varArgs("Node %d Camp-Endposition not set!\n", i));
            return false;
         }
      }
      else if (g_waypoints[i]->flags & WaypointFlags::terrorist)
         iTPoints++;
      else if (g_waypoints[i]->flags & WaypointFlags::counterTerrorist)
         iCTPoints++;
      else if (g_waypoints[i]->flags & WaypointFlags::goal)
         iGoalPoints++;
      else if (g_waypoints[i]->flags & WaypointFlags::rescue)
         iRescuePoints++;

      int x = 0;

      while (x < MAX_PATH_INDEX)
      {
         if (g_waypoints[i]->index[x] != -1)
         {
            if (g_waypoints[i]->index[x] >= g_numWaypoints || g_waypoints[i]->index[x] < -1)
            {
               SERVER_PRINT(util::varArgs("Node %d - Pathindex %d out of Range!\n", i, x));
               g_engfuncs.pfnSetOrigin(g_world->hostEdict, g_waypoints[i]->origin);
               g_waypointOn = true;
               g_editNoclip = true;
               return false;
            }
            else if (g_waypoints[i]->index[x] == i)
            {
               SERVER_PRINT(util::varArgs("Node %d - Pathindex %d points to itself!\n", i, x));
               g_engfuncs.pfnSetOrigin(g_world->hostEdict, g_waypoints[i]->origin);
               g_waypointOn = true;
               g_editNoclip = true;
               return false;
            }
         }
         x++;
      }
   }
   if (g_world->getMapType() & MapTypes::hostageRescue)
   {
      if (iRescuePoints == 0)
      {
         SERVER_PRINT("You didn't set a Rescue Point!\n");
         return false;
      }
   }
   if (iTPoints == 0)
   {
      SERVER_PRINT("You didn't set any Terrorist Important Point!\n");
      return false;
   }
   else if (iCTPoints == 0)
   {
      SERVER_PRINT("You didn't set any CT Important Point!\n");
      return false;
   }
   else if (iGoalPoints == 0)
   {
      SERVER_PRINT("You didn't set any Goal Point!\n");
      return false;
   }

   // 12/4/2004 Whistler
   // Perform a DFS instead of Floyd-Warshall to speed up this progress
   // as the Floyd-Warshall pathfinding is just unnecessary
   bool rgbVisited[MAX_WAYPOINTS];
   game::search::PathNode *stack = NULL;

   // first check incoming connectivity
   // initialize the "visited" table
   for (i = 0; i < g_numWaypoints; i++)
      rgbVisited[i] = false;

   // check from Waypoint nr. 0
   stack = (game::search::PathNode *)malloc(sizeof(game::search::PathNode));
   stack->NextNode = NULL;
   stack->iIndex = 0;

   while (stack)
   {
      // pop a node from the stack
      game::search::PathNode *current = stack;
      stack = stack->NextNode;

      rgbVisited[current->iIndex] = true;

      for (j = 0; j < MAX_PATH_INDEX; j++)
      {
         int iIndex = g_waypoints[current->iIndex]->index[j];
         if (rgbVisited[iIndex])
            continue; // skip this waypoint as it's already visited
         if (iIndex >= 0 && iIndex < g_numWaypoints)
         {
            game::search::PathNode *newnode = (game::search::PathNode *)malloc(sizeof(game::search::PathNode));
            newnode->NextNode = stack;
            newnode->iIndex = iIndex;
            stack = newnode;
         }
      }

      free(current);
   }

   for (i = 0; i < g_numWaypoints; i++)
   {
      if (!rgbVisited[i])
      {
         SERVER_PRINT(util::varArgs("Path broken from Waypoint Nr. 0 to Waypoint Nr. %d!\n", i));
         g_engfuncs.pfnSetOrigin(g_world->hostEdict, g_waypoints[i]->origin);
         g_waypointOn = true;
         g_editNoclip = true;
         return false;
      }
   }

   // then check outgoing connectivity
   std::vector<int> in_paths[MAX_WAYPOINTS]; // store incoming g_waypoints for speedup

   for (i = 0; i < g_numWaypoints; i++)
      for (j = 0; j < MAX_PATH_INDEX; j++)
         if (g_waypoints[i]->index[j] >= 0 && g_waypoints[i]->index[j] < g_numWaypoints)
            in_paths[g_waypoints[i]->index[j]].push_back(i);

   // initialize the "visited" table
   for (i = 0; i < g_numWaypoints; i++)
      rgbVisited[i] = false;

   // check from Waypoint nr. 0
   stack = (game::search::PathNode *)malloc(sizeof(game::search::PathNode));
   stack->NextNode = NULL;
   stack->iIndex = 0;

   while (stack)
   {
      // pop a node from the stack
      game::search::PathNode *current = stack;
      stack = stack->NextNode;

      rgbVisited[current->iIndex] = true;

      for (j = 0; j < (int)in_paths[current->iIndex].size(); j++)
      {
         if (rgbVisited[in_paths[current->iIndex][j]])
            continue; // skip this waypoint as it's already visited
         game::search::PathNode *newnode = (game::search::PathNode *)malloc(sizeof(game::search::PathNode));
         newnode->NextNode = stack;
         newnode->iIndex = in_paths[current->iIndex][j];
         stack = newnode;
      }
      free(current);
   }

   for (i = 0; i < g_numWaypoints; i++)
   {
      if (!rgbVisited[i])
      {
         SERVER_PRINT(util::varArgs("Path broken from Waypoint Nr. %d to Waypoint Nr. 0!\n", i));
         g_engfuncs.pfnSetOrigin(g_world->hostEdict, g_waypoints[i]->origin);
         g_waypointOn = true;
         g_editNoclip = true;
         return false;
      }
   }

   return true;
}



/**
* \brief Adds a path between two waypoints. 
*
* Each waypoint can be connected at most with MAX_PATH_INDEX waypoints. Therefore, 
* if there are already MAX_PATH_INDEX connections with the source waypoint, the 
* function tries to replace the new path with one of greater distance.
*
* \param[in] startIndex The source waypoint index.
* \param[in] endIndex The destination waypoint index.
* \param[in] distance The distance between the waypoints.
**/
void game::waypoints::addPath(short int startIndex, short int endIndex, float distance)
{
   if (startIndex < 0 || startIndex >= g_numWaypoints ||
      endIndex < 0 || endIndex >= g_numWaypoints || startIndex == endIndex)
      return;

   Waypoint *p = g_waypoints[startIndex];
   int i;

   // Don't allow Paths get connected twice
   for (i = 0; i < MAX_PATH_INDEX; i++)
   {
      if (p->index[i] == endIndex)
         return;
   }

   // Check for free space in the connection indices
   for (i = 0; i < MAX_PATH_INDEX; i++)
   {
      if (p->index[i] == -1)
      {
         p->index[i] = endIndex;
         p->distance[i] = abs(distance);
         SERVER_PRINT(util::varArgs("Path added from %d to %d\n", startIndex, endIndex));
         return;
      }
   }

   // There wasn't any free space. Try exchanging it with a long-distance path
   int iMaxDistance = -9999;
   int slot = -1;

   for (i = 0; i < MAX_PATH_INDEX; i++)
   {
      if (p->distance[i] > iMaxDistance)
      {
         iMaxDistance = p->distance[i];
         slot = i;
      }
   }

   if (slot != -1)
   {
      SERVER_PRINT(util::varArgs("Path added from %d to %d\n", startIndex, endIndex));
      p->index[slot] = endIndex;
      p->distance[slot] = abs(distance);
   }
}


/**
* \brief Adds a new waypoint. 
**/
void game::waypoints::add(int flags)
{
   if (FNullEnt(g_world->hostEdict))
      return;

   int index = -1, i;
   float distance;
   Vector v_forward = g_vecZero;
   Waypoint *p = NULL;
   bool bPlaceNew = true;
   Vector vecNewWaypoint = g_world->hostEdict->v.origin;

   game::gui::removeAllAgents();

   g_waypointsChanged = true;

   switch (flags)
   {
   case 6:
      index = game::waypoints::findNearest(g_world->hostEdict->v.origin, 50.0);
      if (index != -1)
      {
         p = g_waypoints[index];
         if (!(p->flags & WaypointFlags::camp))
         {
            ClientPrint(&g_world->hostEdict->v, HUD_PRINTCENTER, "This is no Camping Waypoint!\n");
            return;
         }
         util::math::makeVectors( g_world->hostEdict->v.v_angle );
         v_forward = g_world->hostEdict->v.origin + g_world->hostEdict->v.view_ofs + gpGlobals->v_forward * 640;
         p->campEndX = v_forward.x;
         p->campEndY = v_forward.y;
         // play "done" sound...
         EMIT_SOUND_DYN2(g_world->hostEdict, CHAN_WEAPON, "common/wpn_hudon.wav", 1.0, ATTN_NORM, 0, 100);
      }
      return;

   case 9:
      index = game::waypoints::findNearest(g_world->hostEdict->v.origin, 50.0);
      if (index != -1)
      {
         distance = (g_waypoints[index]->origin - g_world->hostEdict->v.origin).Length();
         if (distance < 50)
         {
            bPlaceNew = false;
            p = g_waypoints[index];
            if (flags == 9)
               p->origin = (p->origin + g_vecLearnPos) / 2;
         }
      }
      else
         vecNewWaypoint = g_vecLearnPos;
      break;

   case 10:
      index = game::waypoints::findNearest(g_world->hostEdict->v.origin, 50.0);
      if (index != -1)
      {
         distance = (g_waypoints[index]->origin - g_world->hostEdict->v.origin).Length();
         if (distance < 50)
         {
            bPlaceNew = false;
            p = g_waypoints[index];
            int flags = 0;
            for (i = 0; i < MAX_PATH_INDEX; i++)
               flags += p->connectflag[i];

            if (flags == 0)
               p->origin = (p->origin + g_world->hostEdict->v.origin) / 2;
         }
      }
      break;
   }

   if (bPlaceNew)
   {
      if (g_numWaypoints >= MAX_WAYPOINTS)
         return;

      // find the next available slot for the new waypoint...
      index = g_numWaypoints;

      g_waypoints[index] = (Waypoint *)malloc(sizeof(Waypoint));
      if (g_waypoints[index] == NULL)
         util::terminateOnError("Memory Allocation Error!");

      p = g_waypoints[index];

      // increment total number of waypoints
      g_numWaypoints++;
      p->pathNumber = index;
      p->flags = 0;

      // store the origin (location) of this waypoint
      p->origin = vecNewWaypoint;
      p->campStartX = 0;
      p->campStartY = 0;
      p->campEndX = 0;
      p->campEndY = 0;

      for (i = 0; i < MAX_PATH_INDEX; i++)
      {
         p->index[i] = -1;
         p->distance[i] = 0;
         p->connectflag[i] = 0;
         p->vecConnectVel[i] = g_vecZero;
      }

      // store the last used waypoint for the auto waypoint code...
      g_vecLastWaypoint = g_world->hostEdict->v.origin;
   }

   // set the time that this waypoint was originally displayed...
   g_rgfWPDisplayTime[index] = 0;

   if (flags == 9)
      g_iLastJumpWaypoint = index;
   else if (flags == 10)
   {
      distance = (g_waypoints[g_iLastJumpWaypoint]->origin - g_world->hostEdict->v.origin).Length();
      game::waypoints::addPath(g_iLastJumpWaypoint, index, distance);
      i = 0;
      while (i < MAX_PATH_INDEX)
      {
         if (g_waypoints[g_iLastJumpWaypoint]->index[i] == index)
         {
            g_waypoints[g_iLastJumpWaypoint]->connectflag[i] |= WaypointConnectionFlags::jump;
            g_waypoints[g_iLastJumpWaypoint]->vecConnectVel[i] = g_vecLearnVelocity;
            break;
         }
         i++;
      }
      game::waypoints::calculateWayzone(index);
      return;
   }

   if (g_world->hostEdict->v.flags & FL_DUCKING)
      p->flags |= WaypointFlags::crouch;  // set a crouch waypoint

   if (g_world->hostEdict->v.movetype == MOVETYPE_FLY)
   {
      p->flags |= WaypointFlags::ladder;
      util::math::makeVectors( g_world->hostEdict->v.v_angle );
      v_forward = g_world->hostEdict->v.origin + g_world->hostEdict->v.view_ofs + gpGlobals->v_forward * 640;
      p->campStartY = v_forward.y;
   }

   switch (flags)
   {
   case 1:
      p->flags |= WaypointFlags::crossing;
      p->flags |= WaypointFlags::terrorist;
      break;

   case 2:
      p->flags |= WaypointFlags::crossing;
      p->flags |= WaypointFlags::counterTerrorist;
      break;

   case 3:
      p->flags |= WaypointFlags::noHostage;
      break;

   case 4:
      p->flags |= WaypointFlags::rescue;
      break;

   case 5:
      p->flags |= WaypointFlags::crossing;
      p->flags |= WaypointFlags::camp;
      util::math::makeVectors( g_world->hostEdict->v.v_angle );
      v_forward = g_world->hostEdict->v.origin + g_world->hostEdict->v.view_ofs + gpGlobals->v_forward * 640;
      p->campStartX = v_forward.x;
      p->campStartY = v_forward.y;
      break;

   case 100:
      p->flags |= WaypointFlags::goal;
      break;
   }

   EMIT_SOUND_DYN2(g_world->hostEdict, CHAN_WEAPON, "weapons/xbow_hit1.wav", 1.0, ATTN_NORM, 0, 100);

   game::waypoints::calculateWayzone(index); // Calculate the wayzone of this waypoint

   if (p->flags & WaypointFlags::ladder) // Ladder waypoints need careful connections
   {
      float min_distance = 9999.0;
      int endIndex = -1;

      // calculate all the g_waypoints to this new waypoint
      for (i = 0; i < g_numWaypoints; i++)
      {
         if (i == index)
            continue;  // skip the waypoint that was just added

         if (g_waypoints[i]->flags & WaypointFlags::ladder) // Other ladder waypoints should connect to this
         {
            TraceResult tr;
            util::traceline(vecNewWaypoint, g_waypoints[i]->origin, ignore_monsters,
               ignore_glass, g_world->hostEdict, &tr);
            if (tr.flFraction == 1.0)
            {
               // check if the two waypoints are on the same ladder
               if ((vecNewWaypoint - g_waypoints[i]->origin).Length2D() < 32)
               {
                  // if so, add the g_waypoints between these two waypoints
                  distance = (g_waypoints[i]->origin - vecNewWaypoint).Length();
                  game::waypoints::addPath(index, i, distance);
                  game::waypoints::addPath(i, index, distance);
               }
            }
         }
         else
         {
            // check if the waypoint is reachable from the new one (one-way)
            if (game::waypoints::isReachable(vecNewWaypoint, g_waypoints[i]->origin) )
            {
               distance = (g_waypoints[i]->origin - vecNewWaypoint).Length();
               if (distance < min_distance &&
                  fabs(g_waypoints[i]->origin.z - vecNewWaypoint.z) < 50.0)
               {
                  endIndex = i;
                  min_distance = distance;
               }
            }
         }
      }

      if (endIndex > -1 && endIndex < g_numWaypoints)
      {
         // check if the waypoint is reachable from the new one (one-way)
         if (game::waypoints::isReachable(vecNewWaypoint, g_waypoints[endIndex]->origin))
         {
            distance = (g_waypoints[endIndex]->origin - vecNewWaypoint).Length();
            game::waypoints::addPath(index, endIndex, distance);
         }

         // check if the new one is reachable from the waypoint (other way)
         if (game::waypoints::isReachable(g_waypoints[endIndex]->origin, vecNewWaypoint))
         {
            distance = (g_waypoints[endIndex]->origin - vecNewWaypoint).Length();
            game::waypoints::addPath(endIndex, index, distance);
         }
      }
   }
   else
   {
      // calculate all the g_waypoints to this new waypoint
      for (i = 0; i < g_numWaypoints; i++)
      {
         if (i == index || index == -1)
            continue;  // skip the waypoint that was just added

         // check if the waypoint is reachable from the new one (one-way)
         if ( util::isReachable(vecNewWaypoint, g_waypoints[i]->origin) )
         {
            Vector v_direction = g_waypoints[i]->origin - vecNewWaypoint;
            v_direction = util::math::vectorToAngles(v_direction);
            util::math::makeVectors(v_direction);
            Vector vecCheck = vecNewWaypoint - gpGlobals->v_right * p->radius;
            if (util::isReachable(vecCheck, g_waypoints[i]->origin))
            {
               vecCheck = vecNewWaypoint + gpGlobals->v_right * p->radius;
               if (util::isReachable(vecCheck, g_waypoints[i]->origin))
               {
                  distance = (g_waypoints[i]->origin - vecNewWaypoint).Length();
                  game::waypoints::addPath(index, i, distance);
               }
            }
         }

         // check if the new one is reachable from the waypoint (other way)
         if ( util::isReachable(g_waypoints[i]->origin, vecNewWaypoint) )
         {
            Vector v_direction = vecNewWaypoint - g_waypoints[i]->origin;
            v_direction = util::math::vectorToAngles(v_direction);
            util::math::makeVectors(v_direction);
            Vector vecCheck = g_waypoints[i]->origin - gpGlobals->v_right * g_waypoints[i]->radius;
            if (util::isReachable(vecCheck, vecNewWaypoint))
            {
               vecCheck = g_waypoints[i]->origin + gpGlobals->v_right * g_waypoints[i]->radius;
               if (util::isReachable(vecCheck, vecNewWaypoint))
               {
                  distance = (g_waypoints[i]->origin - vecNewWaypoint).Length();
                  game::waypoints::addPath(i, index, distance);
               }
            }
         }
      }
   }
}

/**
* \brief Deletes a waypoint.
**/
void game::waypoints::deleteWaypoint()
{
   g_waypointsChanged = true;

   if (g_numWaypoints < 1)
      return;

   game::gui::removeAllAgents();

   int index = game::waypoints::findNearest(g_world->hostEdict->v.origin, 50.0);

   if (index == -1)
      return;

   Waypoint *p_previous = NULL;
   Waypoint *p;

   assert(g_waypoints[index] != NULL);

   if (index > 0)
      p_previous = g_waypoints[index - 1];

   int i, ix;

   for (i = 0; i < g_numWaypoints; i++) // delete all references to Node
   {
      p = g_waypoints[i];
      for (ix = 0; ix < MAX_PATH_INDEX; ix++)
      {
         if (p->index[ix] == index)
         {
            p->index[ix] = -1;  // unassign this path
            p->connectflag[ix] = 0;
            p->distance[ix] = 0;
            p->vecConnectVel[ix] = g_vecZero;
         }
      }
   }

   for (i = 0; i < g_numWaypoints; i++)
   {
      p = g_waypoints[i];

      if (p->pathNumber > index) // if Pathnumber bigger than deleted Node...
         p->pathNumber--;

      for (ix = 0; ix < MAX_PATH_INDEX; ix++)
      {
         if (p->index[ix] > index)
            p->index[ix]--;
      }
   }

   // free deleted node
   free(g_waypoints[index]);
   g_waypoints[index] = NULL;

   // Rotate Path Array down
   for (i = index; i < g_numWaypoints - 1; i++)
      g_waypoints[i] = g_waypoints[i + 1];

   g_numWaypoints--;

   g_rgfWPDisplayTime[index] = 0;

   EMIT_SOUND_DYN2(g_world->hostEdict, CHAN_WEAPON, "weapons/mine_activate.wav", 1.0, ATTN_NORM, 0, 100);
}

/**
* \brief Calculate the wayzone for a specific waypoint. 
*
* A wayzone defines a circle around a waypoint origin. When moving
* towards this waypoint, the agent may stop anywhere in within
* the circle
*
* \param[in] index The waypoint index.
**/
void game::waypoints::calculateWayzone(int index)
{
   Waypoint *p = g_waypoints[index];
   Vector start;
   Vector v_direction;
   TraceResult tr;

   if ((p->flags & (WaypointFlags::ladder|WaypointFlags::goal|WaypointFlags::camp|WaypointFlags::rescue|WaypointFlags::crouch)) || g_learnJumpWaypoint)
   {
      p->radius = 0;
      return;
   }

   int x = 0;
   while (x < MAX_PATH_INDEX)
   {
      if (p->index[x] != -1)
      {
         if (g_waypoints[p->index[x]]->flags & WaypointFlags::ladder)
         {
            p->radius = 0;
            return;
         }
      }
      x++;
   }

   bool bWayBlocked = false;
   for (int iScanDistance = 16; iScanDistance < 128; iScanDistance += 16)
   {
      start = p->origin;
      util::math::makeVectors(g_vecZero);
      v_direction = gpGlobals->v_forward * iScanDistance;
      v_direction = util::math::vectorToAngles(v_direction);
      p->radius = iScanDistance;

      for (float fRadCircle = 0.0; fRadCircle < 180.0; fRadCircle += 5)
      {
         util::math::makeVectors(v_direction);
         Vector vRadiusStart = start - gpGlobals->v_forward * iScanDistance;
         Vector vRadiusEnd = start + gpGlobals->v_forward * iScanDistance;

         util::traceHull(vRadiusStart, vRadiusEnd, ignore_monsters, head_hull, NULL, &tr);

         if (tr.flFraction < 1.0)
         {
            util::traceline( vRadiusStart, vRadiusEnd, ignore_monsters, NULL, &tr );
            if (FClassnameIs(tr.pHit, "func_door") ||
               FClassnameIs(tr.pHit, "func_door_rotating"))
            {
               p->radius = 0;
               bWayBlocked = true;
               break;
            }

            bWayBlocked = true;
            p->radius -= 16;
            break;
         }

         Vector vDropStart = start + gpGlobals->v_forward * iScanDistance;
         Vector vDropEnd = vDropStart - Vector(0, 0, iScanDistance + 60);
         util::traceHull( vDropStart, vDropEnd, ignore_monsters, head_hull, NULL, &tr);
         if (tr.flFraction >= 1.0)
         {
            bWayBlocked = true;
            p->radius -= 16;
            break;
         }

         vDropStart = start - gpGlobals->v_forward * iScanDistance;
         vDropEnd = vDropStart - Vector(0, 0, iScanDistance + 60);
         util::traceHull( vDropStart, vDropEnd, ignore_monsters, head_hull, NULL, &tr);
         if (tr.flFraction >= 1.0)
         {
            bWayBlocked = true;
            p->radius -= 16;
            break;
         }

         vRadiusEnd.z += 34;
         util::traceHull( vRadiusStart, vRadiusEnd, ignore_monsters, head_hull, NULL, &tr );

         if (tr.flFraction < 1.0)
         {
            bWayBlocked = true;
            p->radius -= 16;
            break;
         }

         v_direction.y = util::math::angleNormalize(v_direction.y + fRadCircle);
      }
      if (bWayBlocked)
         break;
   }

   p->radius -= 16;
   if (p->radius < 0)
      p->radius = 0;
}

/**
* \brief Calculates waypoint visibility among each other, from a standing and crouching
* agent stance.
**/
void game::waypoints::calculateVisibility()
{
   TraceResult tr;
   unsigned char byRes;
   unsigned char byShift;

   for (int iCurrVisIndex = 0; iCurrVisIndex < g_numWaypoints; iCurrVisIndex++)
   {
      Vector vecSourceDuck = g_waypoints[iCurrVisIndex]->origin;
      Vector vecSourceStand = g_waypoints[iCurrVisIndex]->origin;

      if (g_waypoints[iCurrVisIndex]->flags & WaypointFlags::crouch)
      {
         vecSourceDuck.z += 12.0;
         vecSourceStand.z += 18.0 + 28.0;
      }
      else
      {
         vecSourceDuck.z += -18.0 + 12.0;
         vecSourceStand.z += 28.0;
      }

      uint16 iStandCount = 0, iCrouchCount = 0;

      for (int i = 0; i < g_numWaypoints; i++)
      {
         // First check ducked Visibility
         Vector vecDest = g_waypoints[i]->origin;
         util::traceline(vecSourceDuck, vecDest, ignore_monsters, NULL, &tr);

         // check if line of sight to object is not blocked (i.e. visible)
         if (tr.flFraction != 1.0 || tr.fStartSolid)
            byRes = 1;
         else
            byRes = 0;

         byRes <<= 1;

         util::traceline(vecSourceStand, vecDest, ignore_monsters, NULL, &tr);

         // check if line of sight to object is not blocked (i.e. visible)
         if (tr.flFraction != 1.0 || tr.fStartSolid)
            byRes |= 1;
/*
         if (byRes != 0)
         {
            vecDest = g_waypoints[i]->origin;

            // First check ducked Visibility
            if (g_waypoints[i]->flags & WaypointFlags::crouch)
               vecDest.z += 18.0 + 28.0;
            else
               vecDest.z += 28.0;

            util::traceline(vecSourceDuck, vecDest, ignore_monsters, NULL, &tr );

            // check if line of sight to object is not blocked (i.e. visible)
            if (tr.flFraction != 1.0)
               byRes |= 2;
            else
               byRes &= 1;

            util::traceline(vecSourceStand, vecDest, ignore_monsters, NULL, &tr );

            // check if line of sight to object is not blocked (i.e. visible)
            if (tr.flFraction != 1.0)
               byRes |= 1;
            else
               byRes &= 2;
         }
*/
         byShift = (i % 4)<<1;
         g_rgbyVisLUT[iCurrVisIndex][i >> 2] &= ~(3 << byShift);
         g_rgbyVisLUT[iCurrVisIndex][i >> 2] |= byRes << byShift; 

         if (!(byRes & 2))
            iCrouchCount++;
         if (!(byRes & 1))
            iStandCount++;
      }

      g_waypoints[iCurrVisIndex]->vis.crouch = iCrouchCount;
      g_waypoints[iCurrVisIndex]->vis.stand = iStandCount;
   }
}

/**
* \brief This is function is invoked once per-frame when waypoint editing is on. 
*
* It does the following:
* - Updates the host HUD with waypoint information
* - Implements auto-waypointing
* - Implements jump waypoints learning
**/
void game::waypoints::editingThink()
{
   if (FNullEnt(g_world->hostEdict))
      return;

   float distance, min_distance;
   Vector start, end;
   int i, index = 0;

   if (g_findWPIndex > 0 && g_findWPIndex < g_numWaypoints)
      util::drawArrow(g_world->hostEdict, g_world->hostEdict->v.origin, g_waypoints[g_findWPIndex]->origin, 20, 0, 255, 255, 0, 200, 0, 1);

   if (g_autoWaypointOn && (g_world->hostEdict->v.flags & (FL_ONGROUND | FL_PARTIALGROUND)))
   {
      // find the distance from the last used waypoint
      distance = (g_vecLastWaypoint - g_world->hostEdict->v.origin).Length();

      if (distance > 128)
      {
         min_distance = 9999.0;

         // check that no other reachable waypoints are nearby...
         for (i = 0; i < g_numWaypoints; i++)
         {
            if (game::waypoints::isReachable(g_world->hostEdict->v.origin, g_waypoints[i]->origin))
            {
               distance = (g_waypoints[i]->origin - g_world->hostEdict->v.origin).Length();

               if (distance < min_distance)
                  min_distance = distance;
            }
         }

         // make sure nearest waypoint is far enough away...
         if (min_distance >= 128)
            game::waypoints::add(0);  // place a waypoint here
      }
   }

   min_distance = 9999.0;

   for (i = 0; i < g_numWaypoints; i++)
   {
      distance = (g_waypoints[i]->origin - g_world->hostEdict->v.origin).Length();

      if (distance < 500 && ((util::fInViewCone(&g_waypoints[i]->origin, g_world->hostEdict) &&
         util::fVisible(g_waypoints[i]->origin, g_world->hostEdict)) || !util::isAlive(g_world->hostEdict) || distance < 50))
      {
         if (distance < min_distance)
         {
            index = i; // store index of nearest waypoint
            min_distance = distance;
         }

         if (g_rgfWPDisplayTime[i] + 1.0 < g_world->getTime())
         {
            if (g_waypoints[i]->flags & WaypointFlags::crouch)
            {
               start = g_waypoints[i]->origin - Vector(0, 0, 16);
               end = start + Vector(0, 0, 32);
            }
            else
            {
               start = g_waypoints[i]->origin - Vector(0, 0, 36);
               end = start + Vector(0, 0, 72);
            }

            if (g_waypoints[i]->flags & WaypointFlags::crossing)
            {
               if (g_waypoints[i]->flags & WaypointFlags::camp)
               {
                  if (g_waypoints[i]->flags & WaypointFlags::terrorist)
                     util::drawBeam(g_world->hostEdict, start, end, 15, 0, 255, 160, 160, 250, 0, 10);
                  else if (g_waypoints[i]->flags & WaypointFlags::counterTerrorist)
                     util::drawBeam(g_world->hostEdict, start, end, 15, 0, 255, 160, 255, 250, 0, 10);
                  else
                     util::drawBeam(g_world->hostEdict, start, end, 15, 0, 0, 255, 255, 250, 0, 10);
               }
               else if (g_waypoints[i]->flags & WaypointFlags::terrorist)
                  util::drawBeam(g_world->hostEdict, start, end, 15, 0, 255, 0, 0, 250, 0, 10);
               else if (g_waypoints[i]->flags & WaypointFlags::counterTerrorist)
                  util::drawBeam(g_world->hostEdict, start, end, 15, 0, 0, 0, 255, 250, 0, 10);
            }
            else if (g_waypoints[i]->flags & WaypointFlags::goal)
               util::drawBeam(g_world->hostEdict, start, end, 15, 0, 255, 0, 255, 250, 0, 10);
            else if (g_waypoints[i]->flags & WaypointFlags::ladder)
               util::drawBeam(g_world->hostEdict, start, end, 15, 0, 255, 0, 255, 250, 0, 10);
            else if (g_waypoints[i]->flags & WaypointFlags::rescue)
               util::drawBeam(g_world->hostEdict, start, end, 15, 0, 255, 255, 255, 250, 0, 10);
            else if (g_waypoints[i]->flags & WaypointFlags::noHostage)
               util::drawBeam(g_world->hostEdict, start, end, 15, 0, 255, 255, 0, 250, 0, 10);
            else
               util::drawBeam(g_world->hostEdict, start, end, 15, 0, 0, 255, 0, 250, 0, 10);

            g_rgfWPDisplayTime[i] = g_world->getTime();
         }
      }
   }

   if (g_fPathTime <= g_world->getTime())
   {
      g_fPathTime = g_world->getTime() + 1.0;

      if (min_distance <= 50) // check if player is close enough to a waypoint and time to draw path...
      {
         if (g_waypoints[index]->flags & WaypointFlags::camp)
         {
            if (g_waypoints[index]->flags & WaypointFlags::crouch)
               start = g_waypoints[index]->origin + Vector(0, 0, 16);
            else
               start = g_waypoints[index]->origin + Vector(0, 0, 36);

            end.z = 0;

            end.x = g_waypoints[index]->campStartX;
            end.y = g_waypoints[index]->campStartY;
            util::drawBeam(g_world->hostEdict, start, end, 15, 0, 255, 0, 0, 250, 0, 10);

            end.x = g_waypoints[index]->campEndX;
            end.y = g_waypoints[index]->campEndY;
            util::drawBeam(g_world->hostEdict, start, end, 15, 0, 255, 0, 0, 250, 0, 10);
         }

         Waypoint *p = g_waypoints[index];

         bool isJumpWaypoint = false;
         for (i = 0; i < MAX_PATH_INDEX; i++)
            if (p->index[i] != -1 && p->connectflag[i] & WaypointConnectionFlags::jump)
               isJumpWaypoint = true;

         char msg[512];
         sprintf(msg, "\n\n\n\n    Waypoint Information:\n\n"
            "      Node %d of %d, Radius: %d\n"
            "      Flags:%s%s%s%s%s%s%s%s%s%s%s%s\n",
            index, g_numWaypoints, (int)p->radius,
            (p->flags == 0 && !isJumpWaypoint) ? " (none)" : "",
            p->flags & WaypointFlags::ladder ? " LIFT" : "",
            p->flags & WaypointFlags::crouch ? " CROUCH" : "",
            p->flags & WaypointFlags::crossing ? " CROSSING" : "",
            p->flags & WaypointFlags::camp ? " CAMP" : "",
            p->flags & WaypointFlags::terrorist ? " TERRORIST" : "",
            p->flags & WaypointFlags::counterTerrorist ? " CT" : "",
            p->flags & WaypointFlags::goal ? " GOAL" : "",
            p->flags & WaypointFlags::ladder ? " LADDER" : "",
            p->flags & WaypointFlags::rescue ? " RESCUE" : "",
            p->flags & WaypointFlags::noHostage ? " NOHOSTAGE" : "",
            isJumpWaypoint ? " JUMP" : "");

         MESSAGE_BEGIN(MSG_ONE, SVC_TEMPENTITY, NULL, g_world->hostEdict);
         WRITE_BYTE(TE_TEXTMESSAGE);
         WRITE_BYTE(4); // channel
         WRITE_SHORT(util::fixedSigned16(0, 1<<13)); // x
         WRITE_SHORT(util::fixedSigned16(0, 1<<13)); // y
         WRITE_BYTE(0); // effect
         WRITE_BYTE(0); // r1
         WRITE_BYTE(255); // g1
         WRITE_BYTE(1); // b1
         WRITE_BYTE(100); // a1
         WRITE_BYTE(255); // r2
         WRITE_BYTE(0); // g2
         WRITE_BYTE(1); // b2
         WRITE_BYTE(100); // a2
         WRITE_SHORT(0); // fadeintime
         WRITE_SHORT(0); // fadeouttime
         WRITE_SHORT(util::fixedUnsigned16(1.1, 1<<8)); // holdtime
         WRITE_STRING(msg);
         MESSAGE_END();

         for (i = 0; i < MAX_PATH_INDEX; i++)
         {
            if (p->index[i] != -1)
            {
               Vector pointOfReference = p->origin;
               Vector waypointOrigin = g_waypoints[p->index[i]]->origin;

               if (p->connectflag[i] & WaypointConnectionFlags::jump) // jump connection
                  util::drawBeam(g_world->hostEdict, pointOfReference, waypointOrigin, 5, 0, 255, 0, 128, 200, 0, 10);
               else if (game::waypoints::areConnected(p->index[i], index)) // 2-way connection
                  util::drawBeam(g_world->hostEdict, pointOfReference, waypointOrigin, 5, 0, 255, 255, 0, 200, 0, 10);
               else // 1-way connection
                  util::drawBeam(g_world->hostEdict, pointOfReference, waypointOrigin, 5, 0, 250, 250, 250, 200, 0, 10);
            }
         }

         start = g_waypoints[index]->origin;
         util::math::makeVectors(g_vecZero);
         int iDistance = g_waypoints[index]->radius;
         Vector v_direction = gpGlobals->v_forward * iDistance;
         v_direction = util::math::vectorToAngles(v_direction);

         for (float fRadCircle = 0.0; fRadCircle < 180.0; fRadCircle += 20)
         {
            util::math::makeVectors(v_direction);
            Vector vRadiusStart = start - gpGlobals->v_forward * iDistance;
            Vector vRadiusEnd = start + gpGlobals->v_forward * iDistance;
            util::drawBeam(g_world->hostEdict, vRadiusStart, vRadiusEnd, 10, 0, 0, 0, 255, 250, 0, 10);
            v_direction.y = util::math::angleNormalize(v_direction.y + fRadCircle);
         }
      }
   }

   if (g_learnJumpWaypoint)
   {
      if (!g_bEndJumpPoint)
      {
         if (g_world->hostEdict->v.button & IN_JUMP)
         {
            game::waypoints::add(9);
            g_fTimeJumpStarted = g_world->getTime();
            g_bEndJumpPoint = true;
         }
         else
         {
            g_vecLearnVelocity = g_world->hostEdict->v.velocity;
            g_vecLearnPos = g_world->hostEdict->v.origin;
         }
      }
      else
      {
         if (((g_world->hostEdict->v.flags & FL_ONGROUND) || g_world->hostEdict->v.waterlevel > 0 ||
            g_world->hostEdict->v.movetype == MOVETYPE_FLY) &&
            g_fTimeJumpStarted + 0.1 < g_world->getTime() && g_bEndJumpPoint)
         {
            game::waypoints::add(10);
            g_learnJumpWaypoint = false;
            g_bEndJumpPoint = false;
         }
      }
   }
}

/**
* \brief Changes the flags of the nearest waypoint to an agent
**/
void game::waypoints::changeFlags(int iFlag, char iMode)
{
   int index = game::waypoints::findNearest(g_world->hostEdict->v.origin, 50.0);

   if (index != -1)
   {
      // Delete Flag
      if (iMode == 0)
         g_waypoints[index]->flags &= ~iFlag;
      else
         g_waypoints[index]->flags |= iFlag;

      // play "done" sound...
      EMIT_SOUND_DYN2(g_world->hostEdict, CHAN_WEAPON, "common/wpn_hudon.wav", 1.0, ATTN_NORM, 0, 100);
   }
   return;
}


/**
* \brief Sets the radius of a waypoint.
*
* \param[in] iRadius The new radius.
**/
void game::waypoints::setRadius(int iRadius)
{
   int index = game::waypoints::findNearest(g_world->hostEdict->v.origin, 50.0);

   if (index != -1)
   {
      g_waypoints[index]->radius = (float)iRadius;
      // play "done" sound...
      EMIT_SOUND_DYN2(g_world->hostEdict, CHAN_WEAPON, "common/wpn_hudon.wav", 1.0, ATTN_NORM, 0, 100);
   }
}


/**
* \brief Find the nearest waypoint to a certain point in space.
*
* \param[in] vec The point of reference.
* \param[in] radius The desired waypoint must be within a certain radius from the point of 
*		reference.
* \return The nearest waypoint to a point of reference within radius units.
**/
int game::waypoints::findNearest(Vector vec, float min_distance)
{
   int index = -1;

   for (int i = 0; i < g_numWaypoints; i++)
   {
      float distance = (g_waypoints[i]->origin - vec).Length();

      if (distance < min_distance)
      {
         index = i;
         min_distance = distance;
      }
   }

   return index;
}

/**
* \brief Returns all waypoints within a radius from a point of reference.
*
* \param[in] pointOfReference The point of reference.
* \param[in] fRadius The radius.
* \param[in] buffer A buffer for holding the waypoint indices.
* \param[in] bufferLength The buffer length.
**/
void game::waypoints::findAllWithinRadius(Vector vecPos, float fRadius, int *pTab, int *iCount)
{
   int iMaxCount = *iCount;
   *iCount = 0;

   for (int i = 0; i < g_numWaypoints; i++)
   {
      if ((g_waypoints[i]->origin - vecPos).Length() < fRadius)
      {
         *pTab++ = i;
         *iCount += 1;
         if (*iCount >= iMaxCount)
            break;
      }
   }

   *iCount -= 1;
}

/**
* \brief Creates a path from the nearest waypoint to an agent to a specific waypoint.
*
* \param[in] endIndex The destination waypoint index.
**/
void game::waypoints::createPath(int iNodeNum)
{
   int waypoint1 = game::waypoints::findNearest(g_world->hostEdict->v.origin, 50.0);

   if (waypoint1 == -1)
      return;

   if (iNodeNum < 0 || iNodeNum >= g_numWaypoints)
   {
      ClientPrint(VARS(g_world->hostEdict), HUD_PRINTCONSOLE, "Waypoint Number out of Range!\n");
      return;
   }

   if (iNodeNum == waypoint1)
   {
      ClientPrint(VARS(g_world->hostEdict), HUD_PRINTCONSOLE, "Cannot connect waypoint to itself!\n");
      return;
   }

   float distance = (g_waypoints[iNodeNum]->origin - g_waypoints[waypoint1]->origin).Length();

   game::waypoints::addPath(waypoint1, iNodeNum, distance);

   // play "done" sound...
   EMIT_SOUND_DYN2(g_world->hostEdict, CHAN_WEAPON, "common/wpn_hudon.wav", 1.0, ATTN_NORM, 0, 100);
}

/**
* \brief Creates a path between two waypoints.
*
* \param[in] startIndex The origin waypoint index.
* \param[in] endIndex The destination waypoint index.
**/
void game::waypoints::createPath(int startIndex, int endIndex)
{
   if (startIndex < 0 || startIndex >= g_numWaypoints ||
      endIndex < 0 || endIndex >= g_numWaypoints)
   {
      ClientPrint(VARS(g_world->hostEdict), HUD_PRINTCONSOLE, "Waypoint Number out of Range!\n");
      return;
   }

   if (startIndex == endIndex)
   {
      ClientPrint(VARS(g_world->hostEdict), HUD_PRINTCONSOLE, "Cannot connect waypoint to itself!\n");
      return;
   }

   float distance = (g_waypoints[endIndex]->origin - g_waypoints[startIndex]->origin).Length();

   game::waypoints::addPath(startIndex, endIndex, distance);

   // play "done" sound...
   EMIT_SOUND_DYN2(g_world->hostEdict, CHAN_WEAPON, "common/wpn_hudon.wav", 1.0, ATTN_NORM, 0, 100);
}

/**
* \brief Removes a path from the nearest waypoint to an agent to a specific waypoint.
*
* \param[in] endIndex The destination waypoint index.
**/
void game::waypoints::removePath(int iNodeNum)
{
   int waypoint1 = game::waypoints::findNearest(g_world->hostEdict->v.origin, 50.0);

   if (waypoint1 == -1)
      return;

   if (iNodeNum < 0 || iNodeNum > g_numWaypoints)
   {
      ClientPrint(VARS(g_world->hostEdict), HUD_PRINTCONSOLE, "Waypoint Number out of Range!\n");
      return;
   }

   int i = 0;
   while (i < MAX_PATH_INDEX)
   {
      if (g_waypoints[waypoint1]->index[i] == iNodeNum)
      {
         g_waypoints[waypoint1]->index[i] = -1;  // unassign this path
         g_waypoints[waypoint1]->connectflag[i] = 0;
         g_waypoints[waypoint1]->vecConnectVel[i] = g_vecZero;
         g_waypoints[waypoint1]->distance[i] = 0;  
      }
      i++;
   }

   // play "done" sound...
   EMIT_SOUND_DYN2(g_world->hostEdict, CHAN_WEAPON, "common/wpn_hudon.wav", 1.0, ATTN_NORM, 0, 100);
}

/**
* \brief Removes a path between two waypoints.
*
* \param[in] startIndex The originIndex.
* \param[in] endIndex The destination waypoint index.
**/
void game::waypoints::removePath(int startIndex, int endIndex)
{
   if (startIndex < 0 || startIndex > g_numWaypoints)
   {
      ClientPrint(VARS(g_world->hostEdict), HUD_PRINTCONSOLE, "Waypoint Number out of Range!\n");
      return;
   }

   if (endIndex < 0 || endIndex > g_numWaypoints)
   {
      ClientPrint(VARS(g_world->hostEdict), HUD_PRINTCONSOLE, "Waypoint Number out of Range!\n");
      return;
   }

   int i = 0;
   while (i < MAX_PATH_INDEX)
   {
      if (g_waypoints[startIndex]->index[i] == endIndex)
      {
         g_waypoints[startIndex]->index[i] = -1;  // unassign this path
         g_waypoints[startIndex]->connectflag[i] = 0;
         g_waypoints[startIndex]->vecConnectVel[i] = g_vecZero;
         g_waypoints[startIndex]->distance[i] = 0;  
      }
      i++;
   }

   // play "done" sound...
   EMIT_SOUND_DYN2(g_world->hostEdict, CHAN_WEAPON, "common/wpn_hudon.wav", 1.0,
      ATTN_NORM, 0, 100);
}


/**
* \brief Checks if two waypoints are connected.
*
* \param[in] indexA The waypoint index A.
* \param[in] indexB The waypoint index B.
**/
bool game::waypoints::areConnected(int a, int b)
{
   int ix = 0;
   while (ix < MAX_PATH_INDEX)
   {
      if (g_waypoints[a]->index[ix] == b)
         return true;
      ix++;
   }
   return false;
}


/**
 * \brief Whether a waypoint is connected to any other waypoint.
 *
 * \param[in] index The waypoint index.
 **/
bool game::waypoints::isConnected(int iNum)
{
   for (int i = 0; i < g_numWaypoints; i++)
   {
      if (i != iNum)
      {
         for (int n = 0; n < MAX_PATH_INDEX; n++)
         {
            if (g_waypoints[i]->index[n] == iNum)
               return true;
         }
      }
   }
   return false;
}

/**
* \brief Whether a waypoint is reachable from a certain point of reference.
*
* \param[in] pointOfReference The point of reference.
* \param[in] waypointOrigin The waypoint location.
* \param[in] bIsInWater Whether the point of reference is underwater.
**/
bool game::waypoints::isReachable(Vector pointOfReference, Vector waypointOrigin, bool bIsInWater)
{
   TraceResult tr;

   float distance = (waypointOrigin - pointOfReference).Length();

   // is the destination close enough?
   if (distance < 400)
   {
      // check if this waypoint is visible...
      util::traceline( pointOfReference, waypointOrigin, ignore_monsters, NULL, &tr );

      // if waypoint is visible from current position (even behind head)...
      if (tr.flFraction >= 1.0)
      {
         if (bIsInWater)
            return true;

         if (waypointOrigin.z > pointOfReference.z + 62.0) // is dest waypoint higher than src? (62 is max jump height)
            return false;  // can't reach this one

         if (waypointOrigin.z < pointOfReference.z - 100.0) // is dest waypoint lower than src?
            return false;  // can't reach this one

         return true;
      }
   }

   return false;
}

/**
* \brief Whether a waypoint is visible from another.
*
* \param[in] startIndex The source waypoint index.
* \param[in] endIndex The destination waypoint index.
**/
bool game::waypoints::isVisible(int startIndex, int endIndex)
{
   unsigned char byRes = g_rgbyVisLUT[startIndex][endIndex >> 2];
   byRes >>= (endIndex % 4) << 1;
   return !((byRes & 3) == 3);
}

/**
* \brief Whether a waypoint is visible from another from a ducking agent stance.
*
* \param[in] startIndex The source waypoint index.
* \param[in] endIndex The destination waypoint index.
**/
bool game::waypoints::isDuckVisible(int startIndex, int endIndex)
{
   unsigned char byRes = g_rgbyVisLUT[startIndex][endIndex >> 2];
   byRes >>= (endIndex % 4) << 1;
   return !((byRes & 2) == 2);
}

/**
* \brief Whether a waypoint is visible from another from a standing agent stance.
*
* \param[in] startIndex The source waypoint index.
* \param[in] endIndex The destination waypoint index.
**/
bool game::waypoints::isStandVisible(int startIndex, int endIndex)
{
   unsigned char byRes = g_rgbyVisLUT[startIndex][endIndex >> 2];
   byRes >>= (endIndex % 4) << 1;
   return !((byRes & 1) == 1);
}


/**
* \brief Gets a path distance.
*
* \param[in] startIndex Start path waypoint.
* \param[in] endIndex End path waypoint.
**/
int game::waypoints::getPathDistance(int startIndex, int endIndex)
{
   return (*(g_pFloydDistanceMatrix + startIndex * g_numWaypoints + endIndex));
}
/**
* \brief Whether an index corresponds to a valid waypoint.
*
* \param[in] index The waypoint index.
**/
bool game::waypoints::isValidIndex(int index){
	return (index >= 0) && (index < g_numWaypoints);
}



/**
* \brief Whether an index corresponds to a goal waypoint.
*
* \param[in] Index the waypoint index.
**/
bool game::waypoints::isGoalWaypoint(int index)
{
	return ((g_waypoints[index]->flags & WaypointFlags::goal) != 0);
}
/**
* \brief Whether an index corresponds to a counter-terrorist waypoint.
*
* \param[in] index The waypoint index.
**/
bool game::waypoints::isCounterTerroristWaypoint(int index)
{
	return ((g_waypoints[index]->flags & WaypointFlags::counterTerrorist) != 0);
}
/**
* \brief Whether an index corresponds to a terrorist waypoint.
*
* \param[in] index The waypoint index.
**/
bool game::waypoints::isTerroristWaypoint(int index)
{
	return ((g_waypoints[index]->flags & WaypointFlags::terrorist) != 0);
}
/**
* \brief Whether an index corresponds to a camp waypoint.
*
* \param[in] index The waypoint index.
**/
bool game::waypoints::isCampWaypoint(int index)
{
	return ((g_waypoints[index]->flags & WaypointFlags::camp) != 0);
}
/**
* \brief Whether an index corresponds to a crouch waypoint.
*
* \param[in] index The waypoint index.
**/
bool game::waypoints::isCrouchWaypoint(int index)
{
	return ((g_waypoints[index]->flags & WaypointFlags::crouch) != 0);
}




